//
// Created by fjy on 2021/12/19.
//

#include <ros/ros.h>
#include "tools/file_manager.hpp"
#include "glog/logging.h"
#include <sensor_msgs/PointCloud2.h>
#include <opencv/cv.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/crop_box.h>
#include "GroundPlaneFit.h"
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

using namespace std;


ros::Subscriber sub_lidarcloud;
ros::Publisher pub_segmented_obj;
ros::Publisher pub_ground;

pcl::PointCloud<pcl::PointXYZI>::Ptr fullPointCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr ProjectedPointCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr SegObjectCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr GroundCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr NoGroundCloud;

pcl::CropBox<pcl::PointXYZI> boundary_box_filter_;


string lidar_topic = "/zvision_lidar_points";
string SegObjPub_topic = "/SegmentedObj";
string GroundPub_topic = "/Ground";
string WORK_SPACE_PATH = "/home/fjy/Desktop/catkin_ws/chucao/Obstacle-Cluster/src/Obstacle-Cluster";

std_msgs::Header cloudHeader;

double CAR_HEIGHT = 0.408;
double vertical_resolution = 35.0/80;
double horizon_resolution = 35.0/80;
double vertical_bottom = -20.0;//-20~30
double horizon_bottom = 35.0;//-60~55 ->35~150
double horizon_FOV = 115;
double vertical_FOV = 50;
double sensorMinimumRange = 0.1;

//int N_SCAN = floor(vertical_FOV/vertical_resolution);
//int Horizon_SCAN = 240;
int N_SCAN = 160;
int Horizon_SCAN = 320;

bool lidarreverse = true;//zvision 雷达是颠倒的
//欧式聚类参数
double toler=0.02; // angle threshold of two points
int min_size=0;  // The data of KITTI is not the original lidar signal, we need to consider those holes when search for nearest point in four directions.
int max_size=1000000;

cv::Mat rangeMat = cv::Mat::zeros(N_SCAN,Horizon_SCAN,CV_32F);
pcl::PointXYZI nanPoint;

std::shared_ptr<GroundPlaneFit> gpfMethod = std::make_shared<GroundPlaneFit>();

void CloudProjection(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr laserCloudIn);
void zlidarCallback(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg);
void PublishCloud();

void zlidarCallback(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg) {

    cloudHeader = laserCloudMsg->header;

    pcl::fromROSMsg(*laserCloudMsg, *fullPointCloud);
    // Remove Nan points
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*fullPointCloud, *fullPointCloud, indices);

    //只要20*20以内的点云
//    std::vector<int> boxIndices_boundary;
//    boundary_box_filter_.setInputCloud(fullPointCloud);
//    boundary_box_filter_.filter(boxIndices_boundary);//
//    boundary_box_filter_.filter(*fullPointCloud);

//    gpfMethod->setInputCloud(fullPointCloud);
//    gpfMethod->filterGround(GroundCloud,NoGroundCloud);
//
//    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

//    tree->setInputCloud(NoGroundCloud);
//
//    std::vector<pcl::PointIndices> cluster_indices;
//    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
//    ec.setClusterTolerance(toler); // 2cm
//    ec.setMinClusterSize(min_size);
//    ec.setMaxClusterSize(max_size);
//    ec.setSearchMethod(tree);
//    ec.setInputCloud(NoGroundCloud);
//    ec.extract(cluster_indices);

//    CloudProjection(fullPointCloud);


    PublishCloud();
}

int main(int argc, char *argv[]) {

    google::InitGoogleLogging(argv[0]);
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    FLAGS_alsologtostderr = true;
    FLAGS_colorlogtostderr = true;
    FLAGS_log_prefix = true;
    FLAGS_logbufsecs = 0;
    ObstacleCluster::FileManager::CreateDirectory(FLAGS_log_dir);

    ros::init(argc, argv, "ObstacleCluster_node");
    ros::NodeHandle nh;

    sub_lidarcloud = nh.subscribe("/zvision_lidar_points_colored", 1,zlidarCallback);
    pub_segmented_obj = nh.advertise<sensor_msgs::PointCloud2>(SegObjPub_topic, 1);
    pub_ground = nh.advertise<sensor_msgs::PointCloud2>(GroundPub_topic, 1);

    boundary_box_filter_.setMin(Eigen::Vector4f(-20, -20, -CAR_HEIGHT-0.2, 1.0));
    boundary_box_filter_.setMax(Eigen::Vector4f(20, 20, 3-CAR_HEIGHT, 1.0));

    fullPointCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    ProjectedPointCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    SegObjectCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    GroundCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
    NoGroundCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());

    nanPoint.x = std::numeric_limits<float>::quiet_NaN();
    nanPoint.y = std::numeric_limits<float>::quiet_NaN();
    nanPoint.z = std::numeric_limits<float>::quiet_NaN();
    nanPoint.intensity = -1;

    ProjectedPointCloud->width = N_SCAN*Horizon_SCAN;
    ProjectedPointCloud->height = 1;
    ProjectedPointCloud->points.resize(N_SCAN*Horizon_SCAN);

    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();
    }

    return 0;
}



void PublishCloud(){

    sensor_msgs::PointCloud2 laserCloudTemp;

    if (pub_ground.getNumSubscribers() != 0){
        pcl::toROSMsg(*GroundCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "zvision_lidar";
        pub_ground.publish(laserCloudTemp);
    }

    if (pub_segmented_obj.getNumSubscribers() != 0){
        pcl::toROSMsg(*ProjectedPointCloud, laserCloudTemp);
//        pcl::toROSMsg(*SegObjectCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "zvision_lidar";
        pub_segmented_obj.publish(laserCloudTemp);
    }
}

void CloudProjection(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr laserCloudIn){
    // range image projection
    float verticalAngle, horizonAngle, range;
    size_t rowIdn, columnIdn, index, cloudSize;
    pcl::PointXYZI thisPoint;

    cloudSize = laserCloudIn->points.size();
    rangeMat = cv::Mat::zeros(N_SCAN,Horizon_SCAN,CV_32F);
    std::fill(ProjectedPointCloud->points.begin(), ProjectedPointCloud->points.end(), nanPoint);

    GroundCloud->clear();
    int PointCnt = 0;
    for (size_t i = 0; i < cloudSize; ++i){

        if(lidarreverse){
            thisPoint.x = -laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = -laserCloudIn->points[i].z;
        }
        ProjectedPointCloud->push_back(thisPoint);

//        -20~30
//        verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
//        rowIdn = (verticalAngle - vertical_bottom) / vertical_resolution;
//
//        if (rowIdn < 0 || rowIdn >= N_SCAN)
//            continue;
//
//        horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
//
////        -60~55 ->35~150
////        columnIdn = -round((horizonAngle-90.0)/horizon_resolution) + Horizon_SCAN/2;
//        columnIdn = (horizonAngle+90-horizon_bottom)/horizon_resolution;
//
//
//        if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
//            continue;
//
//        range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
//        if (range < sensorMinimumRange)
//            continue;
//
//        rangeMat.at<float>(rowIdn, columnIdn) = range;
//
//        thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
//
//        index = columnIdn  + rowIdn * Horizon_SCAN;
//        ProjectedPointCloud->points[index] = thisPoint;
//        ProjectedPointCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
//        PointCnt++;
    }
//    cout<<PointCnt<<endl;
}

